package boids;

import toolib.geom.Intersector;
import toolib.geom.V2f;
import toxi.geom.Vec2D;

public class CollisionBox
{
	private Vec2D position;
	private Vec2D dimension;
	
	private Vec2D vertices[];
	
	public CollisionBox(float _x, float _y, float _w, float _h)
	{
		position = new Vec2D(_x, _y);
		dimension = new Vec2D(_w, _h);
		initVertices();
	}
	
	public CollisionBox(Vec2D _position, float _w, float _h)
	{
		position = new Vec2D(_position);
		dimension = new Vec2D(_w, _h);
		initVertices();
	}
	
	private void initVertices()
	{
		vertices = new Vec2D[4];
		
		vertices[0] = new Vec2D(-0.5f * dimension.x, 0);
		vertices[0].addSelf(position);

		vertices[1] = new Vec2D(0.5f * dimension.x, 0);
		vertices[1].addSelf(position);

		vertices[2] = new Vec2D(0.5f * dimension.x, dimension.y);
		vertices[2].addSelf(position);

		vertices[3] = new Vec2D(-0.5f * dimension.x, dimension.y);
		vertices[3].addSelf(position);
	}
	
	public void update(Vec2D _position, Vec2D _velocity)
	{
		position = _position;
		float angle = _velocity.heading() + 1.5f*(float)Math.PI;
		Vec2D actDimension = new Vec2D(dimension.x, dimension.y*_velocity.magnitude());
		
		vertices[0].set(-0.5f * actDimension.x, 0);
		vertices[0].rotate(angle);
		vertices[0].addSelf(position);

		vertices[1].set(0.5f * actDimension.x, 0);
		vertices[1].rotate(angle);
		vertices[1].addSelf(position);

		vertices[2].set(0.5f * actDimension.x, actDimension.y);
		vertices[2].rotate(angle);
		vertices[2].addSelf(position);

		vertices[3].set(-0.5f * actDimension.x, actDimension.y);
		vertices[3].rotate(angle);
		vertices[3].addSelf(position);
	}
	
	public boolean intersectsCircle(Vec2D _cc, float _cr)
	{
		V2f[] a = Intersector.circleToLine(_cc.x, _cc.y, _cr, vertices[0].x, vertices[0].y, vertices[1].x, vertices[1].y);
		V2f[] b = Intersector.circleToLine(_cc.x, _cc.y, _cr, vertices[1].x, vertices[1].y, vertices[2].x, vertices[2].y);
		V2f[] c = Intersector.circleToLine(_cc.x, _cc.y, _cr, vertices[2].x, vertices[2].y, vertices[3].x, vertices[3].y);
		V2f[] d = Intersector.circleToLine(_cc.x, _cc.y, _cr, vertices[3].x, vertices[3].y, vertices[0].x, vertices[0].y);
		
		if (a != null || b != null || c != null || d != null )
			return true;
		else
			return false;
	}
	
	/**
	 * calculates the intersection between a circle and an infinite ray
	 * and returns to coordinates of the intersecting points (if existent)
	 * 
	 * @param _p1 - point one on line
	 * @param _p2 - point two on line
	 * @param _cc - center of circle
	 * @param _r  - radius of circle
	 * @return null or Vec2D array of length 2 holding the intersection points
	 */
	private Vec2D[] intersectRayCircle(Vec2D _p1, Vec2D _p2, Vec2D _cc, float _r)
	{
		Vec2D line = _p2.sub(_p1);
		float a = line.x * line.x + line.y * line.y;
		float b = 2.0f * (line.x * (_p1.x - _cc.x) + line.y * (_p1.y - _cc.y));
		float c = _cc.x * _cc.x + _cc.y * _cc.y;
		c += _p1.x * _p1.x + _p1.y * _p1.y;
		c -= 2.0f * (_cc.x * _p1.x + _cc.y * _p1.y);
		c -= _r * _r;
		
		float bb4ac = b * b - 4.0f * a * c;
		if (bb4ac < 0)
		{
			return null;
		}
		
		Vec2D[] intersectPoints = new Vec2D[2];
		float mu1 = (-b + (float)Math.sqrt(bb4ac)) / (2 * a);
		float mu2 = (-b - (float)Math.sqrt(bb4ac)) / (2 * a);
		
		intersectPoints[0] = new Vec2D(_p1.x + mu1 * (_p2.x - _p1.x), _p1.y + mu1 * (_p2.y - _p1.y));
		intersectPoints[1] = new Vec2D(_p1.x + mu2 * (_p2.x - _p1.x), _p1.y + mu2 * (_p2.y - _p1.y));
		return intersectPoints;
	}
	
	/**
	 * calculates the intersection between a circle and an infinite ray
	 * and returns to coordinates of the intersecting points (if existent)
	 * 
	 * @param _p1 - point one on line
	 * @param _p2 - point two on line
	 * @param _cc - center of circle
	 * @param _r  - radius of circle
	 * @return null or Vec2D array of length 2 holding the intersection points
	 */
	private Vec2D[] intersectLineCircle(Vec2D _p1, Vec2D _p2, Vec2D _cc, float _r)
	{
		Vec2D line = _p2.sub(_p1);
		float a = line.x * line.x + line.y * line.y;
		float b = 2.0f * (line.x * (_p1.x - _cc.x) + line.y * (_p1.y - _cc.y));
		float c = _cc.x * _cc.x + _cc.y * _cc.y;
		c += _p1.x * _p1.x + _p1.y * _p1.y;
		c -= 2.0f * (_cc.x * _p1.x + _cc.y * _p1.y);
		c -= _r * _r;
		
		float bb4ac = b * b - 4.0f * a * c;
		if (bb4ac < 0)
		{
			return null;
		}
		
		Vec2D[] intersectPoints = new Vec2D[2];
		float mu1 = (-b + (float)Math.sqrt(bb4ac)) / (2 * a);
		float mu2 = (-b - (float)Math.sqrt(bb4ac)) / (2 * a);
		
		intersectPoints[0] = new Vec2D(_p1.x + mu1 * (_p2.x - _p1.x), _p1.y + mu1 * (_p2.y - _p1.y));
		intersectPoints[1] = new Vec2D(_p1.x + mu2 * (_p2.x - _p1.x), _p1.y + mu2 * (_p2.y - _p1.y));
		return intersectPoints;
	}
	
	public Vec2D[] getVertices()
	{
		return vertices;
	}

	/**
	 * @return the position
	 */
	public Vec2D getPosition()
	{
		return position;
	}

	/**
	 * @param _position the position to set
	 */
	public void setPosition(Vec2D _position)
	{
		position = _position;
	}
	
	public void setDimension(float _width, float _height)
	{
		dimension.set(_width, _height);
	}
}
